#ifndef ROBORTS_DECISION_BFS_SEARCH_H
#define ROBORTS_DECISION_BFS_SEARCH_H

#include "../blackboard/blackboard.h"
#include "costmap/costmap_interface.h"
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <queue>
#include <unordered_map>

namespace roborts_decision{
class BfsSearch {
 public:
  BfsSearch(Blackboard* &blackboard):blackboard_(blackboard){
      costmap_2d_ = blackboard->GetCostMap2D();    
      global_costmap_ = blackboard_->GetGlobalCostMap();
      max_iter = 1e5;
      //ros::NodeHandle n;
      //result_pub = n.advertise<geometry_msgs::PoseStamped>("bfs",1000);
    }
  
  ~BfsSearch() = default;
  
  bool Run(){
    if(blackboard_->IsNewGoal()){
        geometry_msgs::PoseStamped res;
        res.header.stamp = ros::Time::now();
        res.header.frame_id = "map";
        bool succ = search(blackboard_->GetGoal().pose.position,res.pose.position);

        if(succ)
        {
             //result_pub.publish(res);
        }
        else
            std::cout<<"failed"<<std::endl;
    }
    return false;
  }

  bool PointAvaible(unsigned int mx,unsigned int my)
  {
     if(mx<costmap_2d_->GetSizeXCell()&&mx>0&&my<costmap_2d_->GetSizeYCell()&&my>0)
        return true;
     else
        return false;
  }

  bool search(geometry_msgs::Point start,geometry_msgs::Point& res)
  {

      unsigned int start_mx,start_my;
      if(!costmap_2d_->World2Map(start.x,start.y,start_mx,start_my))
      {
	        return false;
      }
      std::cout<<"start:"<<blackboard_->ifCurrentValid();
      std::unordered_map<int,bool> visited;
      iter_queue.push(std::pair<unsigned int,unsigned int>{start_mx,start_my});
      int iter_times = 0;

      while(!iter_queue.empty()&&iter_times<max_iter)
      {

        iter_times++;
        auto cur = iter_queue.front();
        iter_queue.pop();
        if(global_costmap_->data[costmap_2d_->GetIndex(cur.first,cur.second)]<2)
        {
            costmap_2d_->Map2World(cur.first,cur.second,res.x,res.y);
            iter_queue = std::queue<std::pair<int,int>>();
            return true;
        }
        else
        {
            visited[costmap_2d_->GetIndex(cur.first,cur.second)] = true;
            std::pair<unsigned int,unsigned int> next;
            next.first = cur.first+1;
            next.second = cur.second;
            //四连通搜索
            if(visited.find(costmap_2d_->GetIndex(next.first,next.second))==visited.end()&&PointAvaible(next.first,next.second))
            {
                iter_queue.push(next);
            }
            next.first = cur.first-1;
            next.second = cur.second;
            if(visited.find(costmap_2d_->GetIndex(next.first,next.second))==visited.end()&&PointAvaible(next.first,next.second))
            {
                iter_queue.push(next);
            }
            next.first = cur.first;
            next.second = cur.second+1;
            //四连通搜索
            if(visited.find(costmap_2d_->GetIndex(next.first,next.second))==visited.end()&&PointAvaible(next.first,next.second))
            {
                iter_queue.push(next);
            }
            next.first = cur.first;
            next.second = cur.second-1;
            if(visited.find(costmap_2d_->GetIndex(next.first,next.second))==visited.end()&&PointAvaible(next.first,next.second))
            {
                iter_queue.push(next);
            }
        }
      }
      return false;
  }

 private:
  Blackboard* const blackboard_;

  const roborts_costmap::Costmap2D* costmap_2d_;
  nav_msgs::OccupancyGrid* global_costmap_;
  std::queue<std::pair<int,int>> iter_queue;
  int max_iter;
  //ros::Publisher result_pub;
};
}
#endif
